#!/usr/bin/env python

import roslib; roslib.load_manifest('team4')
import rospy
from lift_test import Tray 
from geometry_msgs.msg import PointStamped
from ObjectUnstacker import *
from ObjectStacker import *
from PlatePicker import *
from Room3 import *
from free_table_spot import *
from pr2_python import base, gripper

if __name__ == "__main__":
   rospy.init_node("team4_demo")

   rospy.loginfo("Beginning team 4 demo")
   rospy.loginfo("Creating all class instances")
   # instantiate classes here (if they need instantiating)
   tray = Tray()
   base_mover=base.Base()
   stacker = ObjectStacker()
   picker = PlatePicker()
   room3 = Room3()
   ou = ObjectUnstacker()

   ############################################################
   rospy.loginfo("Unstacking tableware")
   # unstacking plates code
   print "Press key to unstack"
   raw_input()
   picker.put_plate_from_stack_on_table()

